package com.rockwell.sniffyhunter.model;

import java.util.Map;

import org.opencv.core.Point;

import com.rockwell.sniffyhunter.utils.DetectionIndex;
import com.rockwell.sniffyhunter.utils.IDetector;
import com.rockwell.sniffyhunter.utils.IRobotPositionDetector;
import com.rockwell.sniffyhunter.utils.IRobotSensors;
import com.rockwell.sniffyhunter.utils.IStateMachine;
import com.rockwell.sniffyhunter.utils.MoveToTreasureState;

public class MoveToTreasure extends StateObject {
	
	private double m_totalDistanceTraveled;
	private double m_totalAngleTurned;
	private double m_initialOrientation;
	private RobotCommandSequencer m_commandSequencer;
	private RobotPosition m_robotPosition;
	private IRobotPositionDetector m_positionDetector;
	private Map<Integer, IDetector> m_colorDetectors;
	private IRobotSensors m_sensors;
	private final static double CONFIRM_TREASURE_LOST_MOVE_BACK_DISTANCE = -5;
	private final static double LOWER_ROTATE_ANGLE = 4;
	private final static double UPPER_ROTATE_ANGLE = 5;
	private final static double LOWER_INDEX_DISTANCE = 20;
	private final static double UPPER_INDEX_DISTANCE = 50;
	private double m_rotateAngle;
	private double indexDistance;
	private boolean m_moveBackToConfirmTreasureLostDone;
	private ExitCode m_exitCode;

	private enum ExitCode {
		UNINITIALIZED,
		DESTINATION_REACHED,
		TREASURE_LOST
	}
	
	public MoveToTreasure(Robot robot) {
		super(robot);
		m_commandSequencer = new RobotCommandSequencer(robot);
		m_positionDetector = robot.m_positionDetector;
		m_colorDetectors = robot.m_colorDetectors;
		m_sensors = robot.m_sensors;
	}

	@Override
	public void Initialize() {
		//m_commandSequencer.Initialize();
		machineState = MoveToTreasureState.START;
		m_totalAngleTurned = 0;
		m_totalDistanceTraveled = 0;
		m_initialOrientation = 0;
	}

	@Override
	public boolean IsDone() {
		return machineState == MoveToTreasureState.DONE;
	}
	
	public double getDistanceTraveled() {
		return m_totalDistanceTraveled;
	}

	public IStateMachine start() {
		m_totalAngleTurned = 0;
		m_totalDistanceTraveled = 0;
		m_initialOrientation = 0;
		m_exitCode = ExitCode.UNINITIALIZED;
		machineState = MoveToTreasureState.CHECK_TREASURE_POS;
		m_moveBackToConfirmTreasureLostDone = false;
		m_initialOrientation = m_sensors.getOrientation();
		return machineState;
	}
	
	public boolean isDestinationReached() {
		return m_exitCode == ExitCode.DESTINATION_REACHED;
	}
	
	public boolean isTresureLost() {
		return m_exitCode == ExitCode.TREASURE_LOST;
	}
	
	public IStateMachine CheckTreasurePos() {

		if (!m_colorDetectors.get(DetectionIndex.TREASURE).isPresent()) {
			if (m_moveBackToConfirmTreasureLostDone) {
				//Treasure lost somehow.
				log("TREASURE_LOST.MoveToTreasure", 1);
				m_exitCode = ExitCode.TREASURE_LOST;
				machineState = MoveToTreasureState.DONE;
			}
			else {
				//Move back to confirm
				machineState = MoveToTreasureState.MOVE_BACK_TO_CONFIRM_TREASURE_LOST;
			}
		}
		else {
			Point treasureCenterDrift = m_colorDetectors.get(DetectionIndex.TREASURE).getDrift();
			
			if (treasureCenterDrift.x == 0 && treasureCenterDrift.y == 0) {
				//Treasure at screen center, destination reached.
				log("DESTINATION_REACHED.MoveToTreasure", 1);
				m_exitCode = ExitCode.DESTINATION_REACHED;
				machineState = MoveToTreasureState.DONE;
			}
			else {
				m_robotPosition = m_positionDetector.getPosition();
				
				Point obsCenter = new Point();
				
				if (m_colorDetectors.get(DetectionIndex.OBSTACLE).isPresent()) {
					obsCenter = m_colorDetectors.get(DetectionIndex.OBSTACLE).getCenter();
				}
				else if (m_colorDetectors.get(DetectionIndex.MAZE1).isPresent()) {
					obsCenter = m_colorDetectors.get(DetectionIndex.MAZE1).getCenter();
				}
				else if (m_colorDetectors.get(DetectionIndex.MAZE2).isPresent()) {
					obsCenter = m_colorDetectors.get(DetectionIndex.MAZE2).getCenter();
				}
				else if (m_colorDetectors.get(DetectionIndex.MAZE3).isPresent()) {
					obsCenter = m_colorDetectors.get(DetectionIndex.MAZE3).getCenter();
				}
				
				Point treasureCenter = m_colorDetectors.get(DetectionIndex.TREASURE).getCenter();
				
				if (m_robotPosition.frontwalldetected && obsCenter.x != 0 && (obsCenter.x > treasureCenter.x)) {

						//Treasure lost as it's blocked by obstacle
						log("treasure blocked by obstacle.MoveToTreasure", 1);
						m_exitCode = ExitCode.TREASURE_LOST;
						machineState = MoveToTreasureState.DONE;

						//Too near to robot, need to reverse.
						//log("Too near.MoveToTreasure", 1);
						//indexDistance = CONFIRM_TREASURE_LOST_MOVE_BACK_DISTANCE;
						//machineState = MoveToTreasureState.INDEX;
				}
				else if (treasureCenterDrift.y != 0) {
					//Treasure not in the screen center, rotate to center
					String msg = String.format("rotate to center[Y.1f].MoveToTreasure", treasureCenterDrift.y);
					log(msg, 1);
					if (treasureCenterDrift.y > 0) {
						if (treasureCenterDrift.y > 0.15f) {
							m_rotateAngle = UPPER_ROTATE_ANGLE;
						}
						else {
							m_rotateAngle = LOWER_ROTATE_ANGLE;
						}
					}
					else {
						if (treasureCenterDrift.y < -0.15f) {
							m_rotateAngle = -UPPER_ROTATE_ANGLE;
						}
						else {
							m_rotateAngle = -LOWER_ROTATE_ANGLE;
						}
					}
					machineState = MoveToTreasureState.ROTATE_ROBOT;
				}
				else {
					//Treasure in the screen center, index closer
					String msg = String.format("index forward[X%.1f].MoveToTreasure", treasureCenterDrift.x);
					log(msg, 1);
					if (treasureCenterDrift.x > 0) {
						if (treasureCenterDrift.x > 0.15f) {
							indexDistance = -UPPER_INDEX_DISTANCE;
						}
						else {
							indexDistance = -LOWER_INDEX_DISTANCE;
						}
					}
					else {
						if (treasureCenterDrift.x < -0.15f) {
							indexDistance = UPPER_INDEX_DISTANCE;
						}
						else {
							indexDistance = LOWER_INDEX_DISTANCE;
						}
					}
					machineState = MoveToTreasureState.INDEX;
				}
			}
		}

		return machineState;
	}
	
	public IStateMachine Index() {
		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.Move, indexDistance);
		
		if (returnVal == ReturnResult.Completed) {
			log("Index.MoveToTreasure", 4);
			m_totalDistanceTraveled += indexDistance;
			machineState = MoveToTreasureState.CHECK_TREASURE_POS;
		}
		
		return machineState;
	}

	public IStateMachine RotateRobot() {
		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.TurnDegree, m_rotateAngle);
		
		if (returnVal == ReturnResult.Completed) {
			log("RotateRobot.MoveToTreasure", 4);
			
			machineState = MoveToTreasureState.CHECK_TREASURE_POS;
		}
		
		return machineState;
	}
	
	public IStateMachine moveBackToConfirmTreasureLost() {
		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.Move, CONFIRM_TREASURE_LOST_MOVE_BACK_DISTANCE);
		
		if (returnVal == ReturnResult.Completed) {
			log("RotateRobot.MoveToTreasure", 4);
			m_totalDistanceTraveled += CONFIRM_TREASURE_LOST_MOVE_BACK_DISTANCE;
			m_moveBackToConfirmTreasureLostDone = true;
			machineState = MoveToTreasureState.CHECK_TREASURE_POS;
		}
		
		return machineState;
	}
	
	private void setAngleTurned() {
		double currentOrientation = m_sensors.getOrientation();
		m_totalAngleTurned = currentOrientation - m_initialOrientation;
		
		if (m_totalAngleTurned > 180)
			m_totalAngleTurned -= 360;
		else if (m_totalAngleTurned < -180)
			m_totalAngleTurned += 360;
	}
	
	public double getTotalAngleTurned() {
		return m_totalAngleTurned;
	}

}
